disp('WARNING: some routines used seem to give errors in older matlab version')
disp('just fix it as you need for 2008 if there are errrors!');
disp('e.g. the debug output setting <iter-detailed> in fminunc(...)');

% generate data and test camera calibration
NN = 30;
Xi = 0.5 * sin(0.2 * 1:NN );
Yi = 0.5 * cos(0.2 * 1:NN );
Zi = 0.01 * (1:NN);
figure(1)
plot3(Xi,Yi,Zi,'.'); title('original 3D points');


% create camera position and direction to point
CamLoc = [2,1,3];
CamPointAt = [0.1,0.2,-0.2];

% generate the matrix following OpenGL convenction
[camAview RR TT] = GetCameraS03MatrixOGL( CamLoc, CamPointAt, 'true cam position',1 ) ;

gTru = camAview;
Xreal = [Xi; Yi; Zi; ones(1,length(Xi))];
PI0 = [eye(3,3) zeros(3,1)];

% focal length and pixel scale. assume square pixels for use with this
% nonlinear optimization function and most encountered real cameras
pixscale = 2;
f = 10; sx = pixscale; sy = pixscale; 

% take as zero -> shift pixels to center initially
ox = 0; oy = 0; 

% intrinsic calibration matrix for pinhole projective camera
Ktru = [f*sx 0 ox; 0 f*sy oy; 0 0 1];

% create the proejcted points:
% image points = intrinsic parameters * projection * extrinsic [R,T] * 3D points
xi = Ktru * PI0 * gTru * Xreal;

lambdatru = xi(3,:);
figure(2)
plot(xi(1,:),xi(2,:),'r.'); title('projection view of points');

PItru = Ktru*PI0*gTru;
lambda0 = PItru(3,4)+randn(1,1)*0.05*PItru(3,4); % estimate of depth of world origin
cam0 = [1;1;1];

disp('estimating with linear method using *known depth for all points*');
[pi0,R,T,K] = CalibrateCamera2(Xreal,xi,PItru);

disp('estimating with initial (linearly estimated) affine model followed by nonlinear optimization using approx. depth and octant of camera location');
[cc,R,T,K] = CalibrateCameraNonLinear(Xreal, xi, lambda0, cam0);

disp('true projection matrix:'); PItru = Ktru*PI0*gTru
disp('(Linear, all known depths and rescaled) estimated projection matrix:'); pi0*lambda0/pi0(3,4)
disp('(Nonlinear, one point approx. depth) estimated projection matrix:'); PIest = K * [R T]

xihat = dlt_inverse( cc, Xreal(1:3,:)' );
figure(2); hold on; plot(xihat(:,1),xihat(:,2),'bo'); 
legend('original point', 'estimated coeffs operating on XYZ input'); hold off




